classdef IMUMeasCorrTest_Cont < matlab.unittest.TestCase
    
    properties (TestParameter)
        Diff2TrueRatio = {0.05};
        OuterIterStepNum = {5};
        InnerIterStepNum = {4};
    end
    
    methods (Test)
        function vldIMUMeasCorr_cont(testCase, Diff2TrueRatio, OuterIterStepNum, InnerIterStepNum)
            % 验证连续形式的比力及角速度修正量计算
            
            %% 参数设置
            nSamp = 1000;
            fBTrue = randi([-20, 20], [3 nSamp]) * 9.8; % 20g
            omegaIBBTrue = randi([-100, 100], [3 nSamp]) / 180 * pi; % 100°/s
            omegaIBBRateTrue = randi([-200, 200], [3 nSamp]) / 180 * pi; % 200°/s^2
            IMUParTrue = genrandimupar(nSamp, true, true);
            
            IMUParTrueEquiv = genrandimupar(nSamp, true);
            [IMUParDiff, IMUParAppr] = genimupardiff(nSamp, IMUParTrue, IMUParTrueEquiv, Diff2TrueRatio);
            
            %% 计算加入误差的输出值并计算比力及角速度近似值
            [~, fTildeS] = accerror_cont(fBTrue, IMUParTrue.acc, omegaIBBTrue, omegaIBBRateTrue, false);
            fTildeOutS = IMUParTrue.acc.KScal0 .* fTildeS;
            [~, omegaTildeIBS] = gyroerror_cont(omegaIBBTrue, IMUParTrue.gyro, fBTrue, false);
            omegaTildeIBOutS = IMUParTrue.gyro.KScal0 .* omegaTildeIBS;
            [fBAppr, omegaIBBAppr] = imuout2in_cont(fTildeOutS, omegaTildeIBOutS, IMUParAppr, omegaIBBRateTrue, int8([1; 1]), OuterIterStepNum, InnerIterStepNum);
            fBCorrRef = fBTrue - fBAppr;
            omegaIBBCorrRef = omegaIBBTrue - omegaIBBAppr;
            
            %% 偏微分算法计算修正量
            [fBCorrCalc_pdiff, omegaIBBCorrCalc_pdiff] = imumeascorr_pdiff_cont(IMUParDiff, IMUParAppr, fBAppr, omegaIBBAppr, omegaIBBRateTrue);
            
            fBCorrErr_pdiff = (fBCorrCalc_pdiff-fBCorrRef)' / 9.8 * 1e6;
            fBCorrRelatErr_pdiff = relatdiff(fBCorrCalc_pdiff, fBCorrRef)' * 100;
            disp('偏微分算法fB修正量误差绝对值最大值（μg）');
            disp(max(abs(fBCorrErr_pdiff), [], 1));
            disp('偏微分算法fB修正量误差绝对值平均值（μg）');
            disp(mean(abs(fBCorrErr_pdiff), 1));
            disp('偏微分算法fB修正量相对误差绝对值平均值（%）');
            disp(mean(abs(fBCorrRelatErr_pdiff), 1));
            testCase.verifyLessThanOrEqual(mean(abs(fBCorrRelatErr_pdiff)), 0.5);
            disp('~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~');
            
            omegaIBBCorrErr_pdiff = (omegaIBBCorrCalc_pdiff-omegaIBBCorrRef)' / pi * 180 * 3600;
            omegaIBBCorrRelatErr_pdiff = relatdiff(omegaIBBCorrCalc_pdiff, omegaIBBCorrRef)' * 100;
            disp('偏微分算法omegaIBB修正量误差绝对值最大值（°/h）');
            disp(max(abs(omegaIBBCorrErr_pdiff), [], 1));
            disp('偏微分算法omegaIBB修正量误差绝对值平均值（°/h）');
            disp(mean(abs(omegaIBBCorrErr_pdiff), 1));
            disp('偏微分算法omegaIBB修正量相对误差绝对值平均值（%）');
            disp(mean(abs(omegaIBBCorrRelatErr_pdiff), 1));
            testCase.verifyLessThanOrEqual(mean(abs(omegaIBBCorrRelatErr_pdiff)), 0.5);
            disp('~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~');
            
            %% 1阶算法计算修正量
            fBCorrCalc_1stod = accerror_cont(fBAppr, IMUParDiff.acc, omegaIBBAppr, omegaIBBRateTrue, true);
            omegaIBBCorrCalc_1stod = gyroerror_cont(omegaIBBAppr, IMUParDiff.gyro, fBAppr, true);
            
            fBCorrErr_1stod = (fBCorrCalc_1stod-fBCorrRef)' / 9.8 * 1e6;
            fBCorrRelatErr_1stod = relatdiff(fBCorrCalc_1stod, fBCorrRef)' * 100;
            disp('1阶算法fB修正量误差绝对值最大值（μg）');
            disp(max(abs(fBCorrErr_1stod), [], 1));
            disp('1阶算法fB修正量误差绝对值平均值（μg）');
            disp(mean(abs(fBCorrErr_1stod), 1));
            disp('1阶算法fB修正量相对误差绝对值平均值（%）');
            disp(mean(abs(fBCorrRelatErr_1stod), 1));
            testCase.verifyLessThanOrEqual(mean(abs(fBCorrRelatErr_1stod)), 5);
            disp('~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~');
            
            omegaIBBCorrErr_1stod = (omegaIBBCorrCalc_1stod-omegaIBBCorrRef)' / pi * 180 * 3600;
            omegaIBBCorrRelatErr_1stod = relatdiff(omegaIBBCorrCalc_1stod, omegaIBBCorrRef)' * 100;
            disp('1阶算法omegaIBB修正量误差绝对值最大值（°/h）');
            disp(max(abs(omegaIBBCorrErr_1stod), [], 1));
            disp('1阶算法omegaIBB修正量误差绝对值平均值（°/h）');
            disp(mean(abs(omegaIBBCorrErr_1stod), 1));
            disp('1阶算法omegaIBB修正量相对误差绝对值平均值（%）');
            disp(mean(abs(omegaIBBCorrRelatErr_1stod), 1));
            testCase.verifyLessThanOrEqual(mean(abs(omegaIBBCorrRelatErr_1stod)), 5);
            disp('~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~');
        end
    end
end